from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *  # Import all GLUT functions
from OpenGL.GLUT.fonts import GLUT_BITMAP_HELVETICA_12  # Explicitly import the font

from readdata import get_coordinates
import math

class TrajectoryAnimation:
    def __init__(self, points_file=None):
        if points_file:
            self.points = get_coordinates(points_file)
        else:
            self.points = [
                (-0.5, -0.5, -0.5),
                (-0.3, -0.2, -0.4),
                (0.0, 0.0, 0.0),
                (0.3, 0.2, 0.4),
                (0.5, 0.5, 0.5)
            ]

        # Calculate the bounding box of the points
        min_x = min(pt[0] for pt in self.points)
        max_x = max(pt[0] for pt in self.points)
        min_y = min(pt[1] for pt in self.points)
        max_y = max(pt[1] for pt in self.points)
        min_z = min(pt[2] for pt in self.points)
        max_z = max(pt[2] for pt in self.points)

        # Update trajectory_extent to fit all points
        self.trajectory_extent = max(max_x - min_x, max_y - min_y, max_z - min_z) / 2.0

        self.angle_x = 0.0  # Rotation angle around the X-axis
        self.angle_y = 0.0  # Rotation angle around the Y-axis

        # Adjust initial camera position to center the points
        self.camera_target = [
            (min_x + max_x) / 2.0,
            (min_y + max_y) / 2.0,
            (min_z + max_z) / 2.0
        ]
        initial_radius = self.trajectory_extent * 3.0  # Increase radius to ensure all points are visible
        self.current_eye_pos = [
            self.camera_target[0] + initial_radius * math.cos(math.radians(self.angle_y)) * math.cos(math.radians(self.angle_x)),
            self.camera_target[1] + initial_radius * math.sin(math.radians(self.angle_y)) * math.cos(math.radians(self.angle_x)),
            self.camera_target[2] + initial_radius * math.sin(math.radians(self.angle_x))
        ]

        self.current_index = 0
        self.timer_interval = 500
        self.scale_factor = 1.0

        self.lerp_factor = 0.1

    def setup_view(self, w, h):
        glutInit()
        glEnable(GL_MULTISAMPLE)
        glClearColor(0.0, 0.0, 0.2, 1.0)
        glViewport(0, 0, w, h)
        glMatrixMode(GL_PROJECTION)
        glLoadIdentity()
        gluPerspective(45, w / h if h != 0 else 1, 0.1, 100.0)
        glMatrixMode(GL_MODELVIEW)

    def update_animation(self):
        self.current_index = (self.current_index + 1) % len(self.points)

    def draw_cone(self, x, y, z):
        # ✅ 固定尺寸（不随 scale_factor 缩放）
        height = 0.12 * self.trajectory_extent
        radius = 0.05 * self.trajectory_extent

        glPushMatrix()
        glTranslatef(x, y, z)
        # Remove rotation since the cone should align with the Z-axis
        glColor4f(1.0, 1.0, 1.0, 1.0)
        quad = gluNewQuadric()
        gluCylinder(quad, 0.0, radius, height, 16, 16)
        glPopMatrix()

    def draw_sphere(self, x, y, z, is_red=False):
        glPushMatrix()
        glTranslatef(x, y, z)
        glColor4f(1.0, 0.0, 0.0, 1.0) if is_red else glColor4f(1.0, 1.0, 1.0, 1.0)
        quad = gluNewQuadric()
        gluSphere(quad, 0.04, 16, 16)  # ✅ 保持常规大小
        glPopMatrix()

        if is_red:
            self.draw_cone(x, y, z)

    def draw_background_cube(self, size=None):
        # Dynamically set the cube size based on trajectory_extent
        size = size or self.trajectory_extent * 3.5  # Increase cube size to fit all points
        hs = size / 2
        vertices = [
            (-hs, -hs, -hs), (hs, -hs, -hs), (hs, hs, -hs), (-hs, hs, -hs),
            (-hs, -hs, hs),  (hs, -hs, hs),  (hs, hs, hs),  (-hs, hs, hs)
        ]
        edges = [
            (0, 1), (1, 2), (2, 3), (3, 0),
            (4, 5), (5, 6), (6, 7), (7, 4),
            (0, 4), (1, 5), (2, 6), (3, 7)
        ]
        glColor4f(0.3, 0.7, 1.0, 0.5)
        glBegin(GL_LINES)
        for edge in edges:
            for vertex in edge:
                glVertex3f(*vertices[vertex])
        glEnd()

    def draw_axes(self, x, y, z):
        # ✅ 固定尺寸（不随 scale_factor 缩放）
        length = 0.15 * self.trajectory_extent

        glPushMatrix()
        glLineWidth(2.0)
        glBegin(GL_LINES)
        glColor3f(1.0, 0.0, 0.0)
        glVertex3f(x, y, z)
        glVertex3f(x + length, y, z)
        glColor3f(0.0, 1.0, 0.0)
        glVertex3f(x, y, z)
        glVertex3f(x, y + length, z)
        glColor3f(0.0, 0.0, 1.0)
        glVertex3f(x, y, z)
        glVertex3f(x, y, z + length)
        glEnd()

        glColor3f(1.0, 1.0, 1.0)
        glRasterPos3f(x + length, y, z)
        for char in b'x':
            glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, char)
        glRasterPos3f(x, y + length, z)
        for char in b'y':
            glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, char)
        glRasterPos3f(x, y, z + length)
        for char in b'z':
            glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, char)
        glLineWidth(1.0)
        glPopMatrix()

    def draw(self, w, h):
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
        glLoadIdentity()

        desired_target_point = self.points[self.current_index]

        # ✅ 使用 scale_factor 控制观察距离（视角放缩）
        radius = self.trajectory_extent * 2.0 / self.scale_factor

        desired_eye_x = desired_target_point[0] + radius * math.cos(math.radians(self.angle_y)) * math.cos(math.radians(self.angle_x))
        desired_eye_z = desired_target_point[2] + radius * math.sin(math.radians(self.angle_x))  # Z-axis is now vertical
        desired_eye_y = desired_target_point[1] + radius * math.sin(math.radians(self.angle_y)) * math.cos(math.radians(self.angle_x))
        desired_eye_pos = [desired_eye_x, desired_eye_y, desired_eye_z]

        for i in range(3):
            self.camera_target[i] += (desired_target_point[i] - self.camera_target[i]) * self.lerp_factor
            self.current_eye_pos[i] += (desired_eye_pos[i] - self.current_eye_pos[i]) * self.lerp_factor

        gluLookAt(self.current_eye_pos[0], self.current_eye_pos[1], self.current_eye_pos[2],
                  self.camera_target[0], self.camera_target[1], self.camera_target[2],
                  0, 0, 1)  # Z-axis is the up vector

        cube_size = self.trajectory_extent * 2.5
        self.draw_background_cube(size=cube_size)

        glColor4f(1.0, 1.0, 1.0, 1.0)
        glLineWidth(3.0)
        glBegin(GL_LINE_STRIP)
        for pt in self.points:
            glVertex3f(*pt)
        glEnd()

        for i, pt in enumerate(self.points):
            if i % 20 == 0 or i == len(self.points) - 1:
                self.draw_sphere(*pt, is_red=(i == self.current_index))

        x, y, z = self.points[self.current_index]
        self.draw_cone(x, y, z)
        self.draw_axes(x, y, z)
